//
// Created by 24087 on 2025/5/11.
//

#include "Queue1.h"
#include "Interface_ROS.h"
const uint8_t MAX_SIZE1 = 8;

void Queue1Init(Queue1 *queue)
{
	queue->data = calloc(MAX_SIZE1, sizeof(QE_DataType1));
	queue->head = 0;
	queue->tail = 0;
	queue->size = 0;
	queue->capacity = MAX_SIZE1;
}

void Queue1Destroy(Queue1 *queue)
{
	if (queue->data)
	{
		free(queue->data);
		queue->data = NULL;
	}
	queue->head = 0;
	queue->tail = 0;
	queue->size = 0;
	queue->capacity = 0;
}

void Queue1Push(Queue1 *queue, const QE_DataType1 data)
{
	if (Queue1Full(queue))
	{
		printf("Queue1 is full\n");
		return;
	}
	queue->data[queue->tail] = data;
	queue->tail = (queue->tail + 1) % queue->capacity;
	queue->size++;
}

bool Queue1Full(const Queue1 *queue)
{
	if (queue->size == queue->capacity)
	{
		return true;
	}
	return false;
}

QE_DataType1 Queue1Pop(Queue1 *queue)
{
	if (Queue1Empty(queue))
	{
		const QE_DataType1 _default = {.data_H = {0}, .data_L = {0}, .uniqueID = -1, .isComplete = false};
		return _default;
	}

	const QE_DataType1 temp = queue->data[queue->head];
	queue->head = (queue->head + 1) % queue->capacity;
	queue->size--;
	return temp;
}

bool Queue1Empty(const Queue1 *queue)
{
	return queue->size == 0;
}
